clc;clear;close all;
path = ginput() * 100.0;

n_seg = size(path, 1) - 1;

ts = zeros(n_seg, 1);
% calculate time distribution based on distance between 2 points
dist = zeros(n_seg, 1);
dist_sum = 0;
T = 25;

t_sum = 0;
for i = 1:n_seg
    dist(i) = sqrt((path(i+1, 1) - path(i, 1))^2 + (path(i+1, 2) - path(i, 2))^2);
    dist_sum = dist_sum + dist(i);
end
for i = 1:n_seg-1
    ts(i) = dist(i) / dist_sum * T;
    t_sum = t_sum + ts(i);
end
ts(n_seg) = T - t_sum;
% or you can simply average the time
% for i = 1:n_seg
%     ts(i) = 1.0;
% end

% d_order = 4;
% trajectory_generation(d_order, path, ts);
for d_order = 1:4
    trajectory_generation(d_order, path, ts);
end
plot(path(1:size(path,1),1),path(1:size(path,1),2),'o','MarkerSize',10);
legend('velocity', 'acceleration', 'jerk', 'snap', 'waypoints');

function trajectory_generation(d_order,path,ts)
    n_poly_perseg = 2*d_order;
    n_order = n_poly_perseg - 1;
    n_seg = size(ts, 1);
    poly_coef_x = MinimumSnapCloseformSolver(path(:, 1), ts, n_seg, n_order);
    poly_coef_y = MinimumSnapCloseformSolver(path(:, 2), ts, n_seg, n_order);

    X_n = [];
    Y_n = [];
    k = 1;
    tstep = 0.01;
    for i=0:n_seg-1
        %#####################################################
        % STEP 4: get the coefficients of i-th segment of both x-axis
        % and y-axis
        Pxi = poly_coef_x(i*n_poly_perseg+1:(i+1)*n_poly_perseg);
        Pxi = flipud(Pxi);
        Pyi = poly_coef_y(i*n_poly_perseg+1:(i+1)*n_poly_perseg);
        Pyi = flipud(Pyi);
        for t=0:tstep:ts(i+1)
            X_n(k)  = polyval(Pxi,t); % 降次排列
            Y_n(k)  = polyval(Pyi,t);
            k = k+1;
        end
    end

%     if 1 == d_order
%         
%     end
    plot(X_n, Y_n ,'LineWidth',2);
%     plot(X_n, Y_n ,'Color',[0 1.0 0],'LineWidth',2);
    hold on
end

function poly_coef = MinimumSnapCloseformSolver(waypoints, ts, n_seg, n_order)
    n_poly_perseg = n_order + 1;
    d_order = n_poly_perseg / 2;    
    start_cond = zeros(d_order,1);
    start_cond(1) = waypoints(1);
    end_cond = zeros(d_order,1);
    end_cond(1) = waypoints(end);
    %#####################################################
    % you have already finished this function in hw1
    Q = getQ(n_seg, n_order, ts);
    %#####################################################
    % STEP 1: compute M
    M = getM(n_seg, n_order, ts);
    %#####################################################
    % STEP 2: compute Ct
    Ct = getCt(n_seg, n_order);
    C = Ct';
    R = C * inv(M)' * Q * inv(M) * Ct;
    R_cell = mat2cell(R, [n_seg+n_order (d_order-1)*(n_seg-1)], [n_seg+n_order (d_order-1)*(n_seg-1)]);
    R_pp = R_cell{2, 2};
    R_fp = R_cell{1, 2};
    %#####################################################
    % STEP 3: compute dF
    %
    %
    %
    %
    dF =  [start_cond; waypoints(2:end-1); end_cond];

    dP = -inv(R_pp) * R_fp' * dF;
    poly_coef = inv(M) * Ct * [dF;dP];
end